#ifndef HEXAPOD_LEG_H
#define HEXAPOD_LEG_H

#include "HexapodServo.h"
#include "HexapodPoint3d.h"

//                   \tail/
//  id14, id16, id18 |body| id17, id15, id13
//  id8, id10, id12  |body| id11,  id9, id7
//  id2,  id4,  id6  |body| id5,  id3,  id1 
//                   \head/
//  


//       \tail/
//  Leg3 |body| Leg2
//  Leg4 |body| Leg1
//  Leg5 |body| Leg0
//       \head/
//  

#define LEG_CNT  6u

// unit: mm
extern const float LegMountLeftRightX;
extern const float LegMountOtherX;
extern const float LegMountOtherY;

extern const float LegRootToJoint1;
extern const float LegJoint1ToJoint2;
extern const float LegJoint2ToJoint3;
extern const float LegJoint3ToTip;


// unit: ms
extern const int movementInterval;
extern const int movementSwitchDuration;

typedef struct _Leg {
	Point3D extrinsics;
	Point3D localPos;
	Point3D bodyPos;
	SERVO servo[3];
	void (*localConv)(const Point3D*, Point3D*);
	void (*bodyConv)(const Point3D*, Point3D*);
} Leg;

void translateBodyToLocal(const Point3D* body, Point3D* local, Leg* leg);
void translateLocalToBody(const Point3D* local, Point3D* body, Leg* leg);

void forwardKinematics(const float angle[3], Point3D* dst);
void inverseKinematics(const Point3D* dst, float angles[3]);

void legInit(uint8_t index, Leg* leg);
void legInitHelp(Leg* leg, const Point3D pt3d);
void setLegAngle(float angle[3], Leg* leg);
void setLegPosition(const Point3D* dst, Leg* leg);
void setBodyTip(const Point3D* dst, Leg* leg);
void setLocalTip(const Point3D* local, Leg* leg);
void test_leg_move();
#endif